.TITLE MOVE

;THE FOLLOWING ROUTINES ARE CALLED THE "RUN-TIME" CODE AND ARE
;RESPONSIBLE FOR PERFORMING THE OPERATIONS OF THE MOTION INSTRUCTIONS.
;AT THE TIME THAT THESE SUBROUTINES ARE CALLED, IT IS ASSUMED THAT R1
;CONTAINS A POINTER TO THE MOTION STEP BEING EXECUTED AND R0 CONTAINS
;A POINTER TO THE NEXT SEQUENTIAL MOTION STEP.  EACH STEP IS STORED
;IN MEMORY IN THE FOLLOWING FORMAT:
;
;		|-----------------------|
;	R1→	| PTR TO NEXT STEP OR 0 |
;		|-----------------------|
;		| PTR TO FUNCTION BLK 	|
;		|-----------------------|
;		|   VARIBLE LENGTH      |
;		|                       |
;		|     DATA AREA         |
;		|-----------------------|
;
;AT THE COMPLETION OF THESE ROUTINES THE ARM STATUS WORD ( "ARMS" )
;CONTAINS FLAG BITS WHICH INDICATE ANY RUN-TIME ERRORS WHICH MIGHT
;HAVE OCCURRED.  IF A RUN-TIME ROUTINE RESULTED IN A REQUEST FOR A
;CHANGE IN THE MOTION INSTRUCTION EXECUTION SEQUENCE ( EG. A BRANCH 
;ON CONDITION ), THE NEXT STEP POINTER IN R0 WILL HAVE BEEN ADJUSTED
;TO THE ABSOLUTE ADDRESS OF OF NEXT STEP TO BE EXECUTED.

;REGISTERS USED:
;	R0,R1 PASS ARGUMENTS AND R1 MAY BE ALTERED
;	ALL OTHER REGISTERS ARE AVAILABLE FOR USE

;DEFINED GLOBALS

.GLOBL GO,GRASP,OPEN,FDRIVE,SPEED,COMNT,FTYPE,PAUSE
.GLOBL RIGHTY,LEFTY,BELOW,ABOVE,FLIP,NOFLIP
.GLOBL FSTOP,COARSE,INTOFF,INTON,PULSE

;REQUIRED GLOBALS

.GLOBL CONFIG,ABELOW,ABBL,FNOF,AFLIP,LOWTOL,MODES,NOINTG
.GLOBL TRNPTR,JANGLE,SOLVE,HAND,LHAND,FINAL,ARMS,CANPRO
.GLOBL HNDDNE,HNDBRK,MARG,USTOP,LSTOP,DRIVE,BADJTN,TYPERR
.GLOBL TYPSTR,EVAL,NSPEED,LINOUT,LFRT,ART,ONINTG,HITHND
.GLOBL NOSOL,NOTDAT,ARMDNE,ARMBRK,ANGLES,BADCLS,UHAND
.GLOBL PULTME,RELBRK,STPARM

;REGISTER DEFINITIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SG	=%5
SP	=%6
PC	=%7
;"GO"     - ARM MOTION IN SLEWING MODE

;THE GO MOTION INSTRUCTION PERFORMS A SLEWING MOTION FROM THE CURRENT
;POSITION OF THE ARM TO ANOTHER LOCATION AND ORIENTATION SPECIFIED BY
;A GIVEN TRANFORMATION.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS
;LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;NO PATH IS COMPUTED FOR THIS MOTION SO THE ARM IS FREE TO GO FROM
;POINT TO POINT IN WHAT EVER FASHION IT CHOOSES.

GO:	MOV	MARG(R1),R1	;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	GONOT
	MOV	R1,@#GOTRN	;SAVE TRANS POINTER
	MOV	#GOLST,R1	;"GO" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	BR	GODNE

GONOT:	MOV	#CANPRO,@#ARMS	;INDICATE ERROR, CAN PROCEED
	MOV	#NOTDAT,R1
	JSR	PC,TYPERR

GODNE:	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

GOSUB:	BIS	#FINAL,16(R1)	;ONLY ONE PASS THRU THIS RTN
	MOV	@#GOTRN,R0	;CONVERT TRANS TO JOINT ANGLES
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;VALID SOLUTION?
	BEQ	GOSDN		;YES
	BIS	#NOSOL,R0	;INDICATE INVALID SOLUTION
	MOVB	R0,@#ARMS
	BIS	#CANPRO,@#ARMS
GOSDN:	RTS	PC
	

;LOCAL STORAGE AREA

GOLST:	ARMDNE	;MASK BITS FOR ARM JOINTS
	625.	;ALLOW 6 SECONDS FOR ARM MOTION
	GOSUB	;32MSEC SUBROUTINE
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

GOTRN:	0	;PTR TO GO TRANSFORM


;END OF "GO"
;"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT

;THE GRASP MOTION INSTRUCTION IS USED FOR CLOSING THE FINGERS AROUND
;OBJECTS.  THE FINGERS ARE COMMANDED TO CLOSE AND THE MOTION IS 
;TERMINATED AFTER A VERY BRIEF PERIOD.  IF THE HAND CLOSES
;FURTHER THAN A SPECIFIED DISTANCE AN ERROR MESSAGE IS SIGNALED.  ITS
;MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE     |
;		|-----------------------|

GRASP:	MOV	R0,-(SP)
	MOV	MARG(R1),R3	;MINIMUM HAND OPENING
	MOV	@#LHAND,@#HAND	;CLOSE THE HAND
	MOV	#GPLST,R1
	JSR	PC,DRIVE
	TST	@#ARMS  	;ANY ERRORS?
	BNE	GPDNE		;EXIT IF YES
      	MOV	#7,R0		;READ THE CURRENT HAND OPENING
	MOV	#1,R1
	JSR	PC,ANGLES
	BCS	GPERR		;BRANCH IF ADC ERROR
	CMP	R3,@#HAND	;HAND OPENING < MINIMUM EXPECTED?
	BLE	GPDNE		;NO ERROR IF GREATER THAN MINIMUM
	BIS	#CANPRO,@#ARMS	;ELSE ERROR
	MOV	#BADCLS,R1
GPERR:	JSR	PC,TYPERR
GPDNE:	MOV	(SP)+,R0
	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

GPSUB:	MOV	R0,R3		;SAVE THE CURRENT SET PT TIME
	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	NOPLSE		;NO
	CMP	@#PULTME,R0	;YES, TIME TO STOP PULSE?
	BGE	NOPLSE		;NO
	BIC	#HITHND,@#MODES	;YES
	MOV	@#MODES,R0
	BIS	#HNDBRK,R0
	JSR	PC,RELBRK
NOPLSE:	CMP	#40.,R3		;TIME TO STOP THE HAND MOTION?
	BGE	.+10		;NO
	BIS	#STPARM,@#ARMS	;YES
	RTS	PC
	

;LOCAL STORAGE AREA

GPLST:	HNDDNE	;MASK BITS FOR HAND IN RANGE
	500.	;ALLOW 5 SECONDS FOR HAND OPERATION
    	GPSUB	;16MSEC SUBROUTINE
	HNDBRK	;BRAKE BITS
	7	;FIRST JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED



;END OF "GRASP" ROUTINES
;"OPEN"   - CHANGES HAND OPENING

;THE OPEN MOTION INSTRUCTION CHANGES THE HAND OPENING.  THIS ROUTINE
;REQUIRES AS ITS ONLY ARGUMENT THE DISTANCE THAT THE HAND IS TO BE
;LEFT OPEN.  NEGATIVE DISTANCES CLOSE THE FINGERS.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE     |
;		|-----------------------|

OPEN:	MOV	MARG(R1),R1	;HAND OPENING
	CMP 	@#UHAND,R1	;GREATER THAN MAX?
	BGE	OPN1		;IN RANGE
	MOV	@#UHAND,R1	;ELSE SET EQUAL TO MAXIMUM
	BR	OPN2
OPN1: 	CMP	@#LHAND,R1	;LESS THAN MIN?
	BLE	OPN2
	MOV	@#LHAND,R1	;SET EQUAL TO MINIMUM
OPN2:	MOV	R1,@#HAND	;SET SET POINT
      	MOV	#OPLST,R1	;POINT TO SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE HAND
       	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

OPSUB:	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	STPOPS		;NO
	CMP	@#PULTME,R0	;YES, TIME TO STOP PULSING?
	BGE	OPSDNE		;NO
	BIC	#HITHND,@#MODES	;YES
	MOV	@#MODES,R0
	BIS	#HNDBRK,R0
	JSR	PC,RELBRK
STPOPS:	BIS	#FINAL,16(R1)	;SIGNAL START CHECKING JOINT DONE
OPSDNE:	RTS	PC


;LOCAL STORAGE AREA

OPLST:	HNDDNE	;MASK BIT  FOR HAND       
	375.	;ALLOW 3 SECONDS FOR HAND OPERATION
	OPSUB	;16MSEC SUBROUTINE
	HNDBRK	;BRAKE BIT
	7	;FIRST JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED


;END OF "OPEN" ROUTINES
;"DRIVE"  - SINGLE JOINT MOTION

;THE DRIVE MOTION INSTRUCTION IS USED TO DRIVE A SINGLE JOINT OF THE
;ARM.  IT REQUIRES THREE ARGUMENTS: THE JOINT TO BE DRIVEN(1-7), THE
;TOTAL CHANGE IN JOINT ANGLE TO PERFORM ( OR HAND OPENING ), AND THE
;TOTAL TIME THE OPERATION IS TO TAKE.  ITS MOTION STEP VARIABLE
;STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|  JOINT NUMBER (1-7)   |
;		|-----------------------|
;		| CHANGE IN JT ANG(DEG) |
;		|-----------------------|
;		|      CLOCK TICKS      |
;		|-----------------------|


FDRIVE:	ADD	#MARG,R1	;PTR TO ARGUMENTS
	MOV	(R1)+,R2     	;JOINT NUMBER
	BLE	BADJT		;MUST BE IN RANGE
	CMP	#7,R2
	BLT	BADJT
	MOV	R2,@#FDRVJT	;SET JOINT TO BE DRIVEN
	DEC	R2		;COMPUTE THE BRAKE BIT MASK
	MOV	#1,R3
	ASH	R2,R3
	MOV	R3,@#FDRVBK	;SAVE THE BRAKE BIT
	SWAB	R3		;GET THE IN RANGE BIT MASK
	MOV	R3,@#FDRVLT	;SAVE IN RANGE MASK BIT
	ASL	R2		;GET JOINT WORD INDEX
	MOV	R2,@#FWINDX	;SAVE INDEX
	MOV	(R1)+,R3	;GET THE CHANGE IN JOINT ANGLE
	CMP	#7,@#FDRVJT	;IF HAND, CONVERT NUMBER TO DISTANCE
	BNE	CHKCHG
	MOV	R3,R4		;SAVE SIGN
	BPL	.+4
	NEG	R4
	MUL	#45.,R4
	ASHC	#-2,R4
	TST	R4		;OVERFLOW?
	BEQ	.+6
	MOV	USTOP(R2),R5	;YES USE MAXIMUM HAND OPENING
	TST	R3		;ATTACH SIGN
	BPL	.+4
	NEG	R5
	MOV	R5,R3
CHKCHG:	ADD	JANGLE(R2),R3	;CHECK IF GOING BEYOND STOP LIMITS
	BVC	NOOVER
	MOV	#100000,R3	;OVERFLOW, SET TO EXTREME ANGLE
	TST	JANGLE(R2)
	BMI	.+6
	MOV	#77777,R3
NOOVER:	CMP	LSTOP(R2),R3	;LESS THAN LOWER?
	BLE	FDRV1
	MOV	LSTOP(R2),R3	;USE LOWER LIMIT
	BR	FDRV2
FDRV1:	CMP	USTOP(R2),R3	;MORE THAN UPPER LIMIT?
	BGE	FDRV2
	MOV	USTOP(R2),R3	;USE UPPER LIMIT
FDRV2:	MOV	R3,@#FTH	;SAVE FINAL JOINT ANGLE
	SUB	JANGLE(R2),R3	;SAVE TOTAL CHANGE
	MOV	R3,@#DTH
	MOV	(R1)+,R2	;GET DURATION FOR MOTION
	BGT	.+6
	MOV	#1,R2		;DEFAULT = 1 TICK
	MOV	R2,@#TIME1	;SAVE IT
	ADD	#375.,R2	;ALLOW 3 SECONDS TO NULL ERROR
	BVC	.+6		;CHECK FOR OVERFLOW
	MOV	#77777,R2	;REPLACE WITH MAXIMUM PERMITED TIME
	MOV	R2,@#FDRVTM	;SAVE TOTAL TIME FOR MOTION
	MOV	#FDRVLT,R1	;PERFORM MOTION
	JSR	PC,DRIVE
	BR	FDRDNE

BADJT:	MOV	#BADJTN,R1	;INDICATE SPECIFICATION ERROR
	JSR	PC,TYPERR
	BIS	#CANPRO,@#ARMS

FDRDNE:	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

FDRVSB:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	MOV	@#TIME1,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MUL 	@#DTH,R0	;x CHANGE IN JOINT ANGLE
	ASHC	#2,R0
	ADD 	@#FTH,R0 	;+ FINAL SET POINT
	ADD	@#FWINDX,R5	;PTR TO JOINT ANGLE SET POINT
	MOV	R0,(R5)		;SAVE NEW SET POINT
	RTS	PC


;LOCAL STORAGE AREA

FDRVLT:	0       ;MASK BITS FOR ARM JOINT DONE BIT
FDRVTM:	0    	;TIME ALLOWED FOR MOTION
	FDRVSB	;SUBROUTINE TO EVALUATE POLYNOMIAL EACH 10 MSEC.
FDRVBK: 0     	;BRAKE BIT 
FDRVJT:	0	;NUMBER OF JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED

FWINDX:	0	;WORD INDEX INTO JANGLE TABLE
TIME1:	0	;DURATION OF MOTION
DTH:	0	;CHANGE IN JOINT ANGLE
FTH:	0	;FINAL JOINT ANGLE

;END OF "DRIVE"
;"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED

;THE SPEED INSTRUCTION IS USED FOR SPECIFYING THE SPEED AT WHICH
;THE NEXT ARM MOTION IS TO BE PERFORMED.  THE SPEED IS EXPRESSED
;AS A PERCENTAGE RANGING FROM 1 (VERY SLOW) TO 32678.( AS FAST
;AS THE HARDWARE SERVO CAN GO).  THE NORMAL OPERATING SPEED IS
;SET TO 100.  THE MOTION STEP VARIABLE STORAGE AREA FOR THIS
;INSTRUCTION IS AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER PERCENTAGE  |
;		|-----------------------|

SPEED:	MOV	MARG(R1),R1	;% SPEED
	CMP	#1,R1		;IF SPEED ≤ 1% USE MINIMUM SPEED
	BGE	USEMIN
	CLR	R2		;NORMAL SPEED IS 100
	MOV	#144000,R3
	DIV	R1,R2		;FRACTIONAL CHANGE
	BR	.+6
USEMIN:	MOV	#77777,R2	;USE MINIMUM SPEED
	MOV	R2,@#NSPEED	;SAVE FOR "TIMER" ROUTINE
	RTS	PC

;END OF "SPEED"
;"PAUSE","TYPE","COMMNT"

;THE FOLLOWING MOTION INSTRUCTIONS TAKE AS THEIR ONLY ARGUMENT A
;STRING.  THE STRINGS ARE STORED IN THE STEP VARIABLE AS FOLLOWS:
;
;		|-----------------------|
;		| 2ND CHAR  |  1ST CHAR |
;		|-----------------------|
;		|           :           |
;		|-----------------------|
;		|     0     | LAST CHAR	|
;		|-----------------------|
;
;A ZERO BYTE IS USED TO MARK THE END OF THE STRING.

;"COMMNT" - DOESN'T DO ANYTHING, NO OPERATION

COMNT: 	RTS	PC


;"TYPE" - TYPES OUT MESSAGE STRING

FTYPE:	MOV	R1,SG
	ADD	#MARG,SG
	JSR	PC,LINOUT
	RTS	PC


;"PAUSE" - TYPES OUT MESSAGE AND HALTS USER PROGRAM EXECUTION

PAUSE:	MOV	R1,SG   	;GET POINTER TO MESSAGE STRING
	ADD	#MARG,SG
	JSR	PC,TYPSTR	;TYPE MESSAGE
	MOV	#PASC1,SG	;TYPE CRLF AND "PAUSE"
	JSR	PC,TYPSTR
	MOV	#CANPRO,@#ARMS	;INDICATE STOP EXECUTION, CAN PROCEED
	RTS	PC

PASC1:	.BYTE	15,12
       	.ASCII	/PAUSE:  /
	.BYTE	0
	.EVEN

;END OF "PAUSE","TYPE","COMMNT"
;"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"

;THE FOLLOWING INSTRUCTIONS ARE FOR ASSERTING THAT THE CONFIGURATION
;OF THE ARM IS TO BE CHANGED DURING THE NEXT ARM MOTION THAT IS NOT A
;"MOVE".  NONE OF THESE INSTRUCTIONS HAVE VARIABLE STOP AREAS.

;"RIGHTY/LEFTY" - RIGHT OR LEFT ARM CONFIGURATION

RIGHTY:	BIS	#LFRT,@#CONFIG
	BIS	#ART,@#CONFIG
	RTS	PC

LEFTY:	BIC	#ART,@#CONFIG
	BIS	#LFRT,@#CONFIG
	RTS	PC

;"ABOVE/BELOW" - APPROACH OBJECTS FROM ABOVE OR BELOW

BELOW:	BIS	#ABBL,@#CONFIG
	BIS	#ABELOW,@#CONFIG
	RTS	PC

ABOVE:	BIC	#ABELOW,@#CONFIG
	BIS	#ABBL,@#CONFIG
	RTS	PC

;"NOFLIP/FLIP" - FLIP OUTER JOINTS, MAKES THETA 5 POSITIVE/NEGATIVE

FLIP:	BIS	#FNOF,@#CONFIG
	BIS	#AFLIP,@#CONFIG
	RTS	PC

NOFLIP:	BIC	#AFLIP,@#CONFIG
	BIS	@#FNOF,@#CONFIG
	RTS	PC

;"STOP","COARSE","INTOFF","INTON","PULSE"

;THE FOLLOWING MOTION INSTRUCTIONS DO NOT TAKE ANY ARGUMENTS AND AS
;SUCH THEY DO NOT HAVE A VARIABLE STORAGE AREA IN THEIR MOTION STEP
;BLOCKS.

;"STOP" - MARKS THE END OF A PASS THROUGH A PROGRAM

FSTOP:	CLR	R0		;TERMINATE WITH NEXT STEP
	RTS	PC


;"COARSE" - SETS LOW TOLERANCE MODE FOR SIGNALING JOINTS IN RANGE

COARSE:	BIS	#LOWTOL,@#MODES
	RTS	PC


;"INTOFF" - SETS MODE TO TURN OFF HARDWARE INTEGRATION FEATURE

INTOFF:	BIS	#NOINTG,@#MODES
	RTS	PC


;"INTON" - SETS MODE TO FORCE HARDWARE INTEGRATION ON ALL OF THE TIME

INTON: 	BIS	#ONINTG,@#MODES
	RTS	PC

;"PULSE" - SETS MODE FOR HARDWARE PULSING OF HAND FOR A FEW MSEC

PULSE:	BIS	#HITHND,@#MODES
	RTS	PC

;END OF RUN-TIME ROUTINES

.END